--*****************************参数设置**********************************--
local ZAxisParam = {
    downPos = 3,      --下降位置
    upPos = 5,        --抬升位置
    Speed = 1000,     --速度mm/s
    Acc = 10,         --加速度m/s^2
    Hold = 0.1,       --保持电流A
    Electricity = 0.2 --电流A
}
--*****************************参数设置**********************************--


local Sensor = ImportModel("SensorBase")
local Count = 100
Sensor.ForcePort = "COM5"
Sensor.ForceBaud = 115200
local maxPos = {}
local forceId = { 3, 4, 5 }
local forceValue = {}

function hex_to_float(hex)
    local i = tonumber(hex, 16)                                             -- 将十六进制数转换为整数
    local s = ((i >> 31) == 0) and 1 or -1                                  -- 提取符号位
    local e = (i >> 23) & 0xFF                                              -- 提取指数部分
    local m = (e == 0) and (i & 0x7FFFFF) << 1 or (i & 0x7FFFFF) | 0x800000 -- 提取尾数部分
    return s * (2 ^ (e - 127)) * (m / 8388608.0)                            -- 计算浮点数
end

function AutoFindPos(jodell)
    jodell:Enable(false)
    jodell:CloseCia402()
    jodell:SetElectricity(2000)
    jodell:SetPositionMode(-51)
    Delay(100)
    jodell:SetControl(0x0F)
    Delay(500)
    jodell:SetControl(0x10F)
    while true do
        local flag = jodell:GetStatus()
        if flag & 0x3000 == 0x3000 then
            break
        end
    end
    local maxPos = jodell:GetParam(0x309C, 32)
    return maxPos
end

function ForceModeInit(maxPos, jodell)
    jodell:SetControl(0x0F)
    jodell:SetPositionMode(-50)
    jodell:SetNoCia402Param(ZAxisParam.downPos, ZAxisParam.upPos, ZAxisParam.Speed, ZAxisParam.Acc, ZAxisParam.Hold,
        ZAxisParam.Electricity)
end

function DownPos(jodell, en)
    jodell:SetControl(0x10F)
    if en then
        while true do
            local flag = jodell:GetStatus()
            if flag & 0x3000 == 0x3000 then
                break
            end
        end
    end
end

function UpPos(jodell, en)
    jodell:SetControl(0x0F)
    if en then
        while true do
            local flag = jodell:GetStatus()
            if flag & 0x3000 == 0x3000 then
                break
            end
        end
    end
end

local xx, slaveValue = CardMaster:GetSlaveValue(0)
if xx ~= 0 then
    print("获取Ethercat从站错误")
end

print(slaveValue)

local jodell = {}

for i = 1, slaveValue, 1 do
    jodell[i] = JodellControl(CardMaster, i - 1)
end

for i, v in ipairs(jodell) do
    maxPos[i] = AutoFindPos(v)
    maxPos[i] = hex_to_float(string.format("%X", maxPos[i]))
    print(string.format("从站:%d 最大位置%.3f", i, maxPos[i]))
end

for i, v in ipairs(jodell) do
    ForceModeInit(maxPos[i], v)
end

for i = 1, Count, 1 do
    -- print("下壓")
    for i, v in ipairs(jodell) do
        DownPos(v, false)
    end
    for i, v in ipairs(jodell) do
        DownPos(v, true)
    end
    Delay(1000)

    for i, v in ipairs(forceId) do
        forceValue[i] = Sensor.GetForce(v)
    end

    print(table.concat(forceValue, " "))

    -- print("抬升")
    for i, v in ipairs(jodell) do
        UpPos(v, false)
    end
    for i, v in ipairs(jodell) do
        UpPos(v, true)
    end
end
